#include <ros/ros.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2/LinearMath/Quaternion.h>

/* 
    需求：发布两个坐标系的相对关系 
    例子：雷达相对于主体的位置
            主体：（0， 0， 0）
            雷达：（0.2 ， 0.0， 0.5）
        获取雷达相对于主体的位置
    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.设置编码
        4.创建发布者对象
        5.组织被发布的消息
        6.发布数据
        7.spin()
*/

int main(int argc, char *argv[])
{
    // 初始化
    ros::init(argc, argv, "coordinate");
    ros::NodeHandle nh;
    // 创建发布者对象
    tf2_ros::StaticTransformBroadcaster pub;
    // 组织被发布的消息
    geometry_msgs::TransformStamped tfs;    // 传输坐标系相关位置信息
    tfs.header.stamp = ros::Time::now();    // 时间戳
    tfs.header.frame_id = "base_link";   // 设置参考对象（主体）id
    tfs.child_frame_id = "laser";       // 设置雷达id
    // 相对主体位置
    tfs.transform.translation.x = 0.2;
    tfs.transform.translation.y = 0.0;
    tfs.transform.translation.z = 0.5;
    tf2::Quaternion qtn;    // 创建四元数对象
    // 相对主体的欧拉角转换（弧度）
    qtn.setRPY(0, 0, 0);    //  翻滚值 俯仰值 偏航值
    // 四元数设置（需要欧拉角转换）
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();

    // 发布数据
    pub.sendTransform(tfs);     // 广播器发布坐标系信息 broadcaster.sendTransform(ts);

    // spin()
    ros::spin();
    
    return 0;
}
